#ifndef __SG_PCL_RANSAC_H__
#define __SG_PCL_RANSAC_H__

#include "SGPCLJsonConfig.h"

#include <utility>

#pragma warning(push)
#include <Eigen/Sparse>
#include <Eigen/Dense>
#pragma warning(pop)
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h> // 拟合平面

namespace sgpcl
{

/// <summary>
/// 点云文件读取算法参数(基于平均距离）
/// </summary>
struct SGRANSACConfig : public SGPCLJsonConfig
{
  float fFluctuation; // 起伏阈值（如果大于此阈值，则该点不在目标模型上)

  SGPCL_API SGRANSACConfig();

  SGPCL_API ~SGRANSACConfig() = default;

  /// <summary>
  /// 解析Json字符串，读取参数
  /// </summary>
  /// <param name="sJsonRoot">Json节点</param>
  SGPCL_API void ParseJson(const rapidjson::Value& sJsonRoot) final override;
};

/// <summary>
/// 读取点云文件，后期有更复杂的功能需求的话考虑写成类
/// </summary>
/// <param name="sConfig">点云文件读取参数</param>
template <typename PointT>
std::pair<Eigen::VectorXf, pcl::PointIndicesPtr> RANSAC(
  typename pcl::PointCloud<PointT>::Ptr spCloud, const SGRANSACConfig& sConfig)
{
  // 计时器
  SG_TIMER("RANSAC");

  // RANSAC拟合平面
  pcl::SampleConsensusModelPlane<PointT>::Ptr model_plane(
    new pcl::SampleConsensusModelPlane<PointT>(spCloud));
  pcl::RandomSampleConsensus<PointT> ransac(model_plane);
  ransac.setDistanceThreshold(sConfig.fFluctuation); // 设置距离阈值
  ransac.computeModel();                               // 执行模型估计

  // 输出模型参数
  Eigen::VectorXf coefficient;
  ransac.getModelCoefficients(coefficient);

  // 筛掉平面外的点
  pcl::PointIndicesPtr inliers(new pcl::PointIndices);
  ransac.getInliers(inliers->indices);

  return { coefficient, inliers };
}

}
#endif // __SG_PCL_RANSAC_H__
